@chanhhoang99, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

See my comment in huynhduc9905#6 (comment)

Codecov Report

All modified and coverable lines are covered by tests ✅

... and 7 files with indirect coverage changes

Your compiler warnings are odd - I just opened a PR that should address it #5277. I'll merge once CI passes in the morning and then you should rebase on it to get the Build Against Released Distributions" jobs to pass

Merged - please rebase!

I’m not sure what’s going on with mypy linting - feel free to ignore that for the scope of this PR if we get everything else passing.

@chanhhoang99 please sign off with DCO :-) That's the only thing blocking for merge other than asking others to validate since this is a major change (even though its like 3 changes 😆 )

Just waiting on testing BTW!

I'm having a hard time finding beta testers with similar situations that aren't too underwater to test this. I'm going to go ahead and merge this after some preliminary testing done be 2-3 people and didn't notice an issue. I'll just keep a look out on the issue tracker if there are any regressions reported.

Thanks for this contribution @chanhhoang99 !

@chanhhoang99 should we keep chatting about the lag compensation piece?

@SteveMacenski , I have not yet make progress on that. I will create another issue when there is improvement.

Great thanks for the update! Let me know if I can be of help! I really appreciate you digging into MPPI, not many folks have given me critical feedback on the internal functioning to date (only that they like the outputs)

For me these changes are breaking; the acceleration limits are no longer respected:

before this PR

after this PR

I'm not too familiar with this part to debug, can you guys see an issue with it? I created #5464 to track

@chanhhoang99 note that I'm currently thinking about this again on a related topic and I as before think that this is correct (though had to revert since it broke people).

On some analysis, I think the issue with the wobbling is due to the control sequence being based on the unsmoothed cvx/cwz vs the previously acceleration bounded versions. That makes the output trajectory more unstable.

The reason that Open Loop odometry seemed to work is since any error / lag in execution is going to amplify the issues, but I don't think that actually solves the core issue at play.

Thus I think the answer is to spend some time on improving the pipeline so that this is more stable with different sampling distributions/spaces and/or post-processing. I have ideas but so far nothing tested to give you concrete ideas.

@chanhhoang99 @adivardi

Check out my notes / progress here: https://github.com/ros-navigation/navigation2/blob/low_accel/low_accel_notes.md

I'm curious on your thoughts, but I think that the open-loop solution is actually a hack that really shouldn't work, but it makes the problem less amplified. I've found the actual no-kidding solution. The next step I need to do is improve MPPI's smoothness to make sure that this is all a net-positive for all users (though it probably already is).

I do not see the wobble issue that you report, but it is very likely a manifestation of some other issues I had to resolve to get the open-loop / closed-loop to work properly with acceleration handling at t=0. I suspect if you try my solution that may just go away. There's still the matter of reducing the jitter in the velocities with better smoothing / sampling distribution / etc, but that's a different topic while I'll start exploring next week.

Please take a look and let me know. Sorry about the delay, I can't always personally jump in at the time when users file issues :( I am doing so now and really digging into it.

@SteveMacenski Do you still have emails that I sent you which have video recordings/yaml file when I was testing with various accel, model_dt, ... changes ? I think they could help understand situation a little bit.
(Those are for the inital PR unclamp the cxv not the open_loop PR.)

I can't seem to find it, can you forward it back to me again? If there's any color you can give me with the context of my current work (things to check/test) that would be appreciated

Fowarded

I think this sums up well a few things I also noticed. In particular:

• Reading the original MPPI paper I also concluded that Unclamp noise velocity. #5266 is theoretically correct
• use open_loop mode violates acceleration limits between iterations: Also noticed that, though it is not causing much issues as our drive motor & steering are really slow, so they "smooth it out" (good to fix though)
• Add acceleration constraints in applyControlSequenceConstraints() relative to the starting speed to ensure that the trajectory is feasible to execute from the current state as well as within the optimal trajectory : I added these to our branch for sanity a while ago, though I didn't see a noticeable change.
• Essentially the shifting logic 'skips' the first control sample vx(0) and applies vx(1) : I still don't get why we do that like we discussed in another PR, but changing it didn't make a noticeable change 😅
• At the time before the open_loop was added (but with the unclamped controls), I also tried playing with gamma & temperature a bit and didn't get any improvement

I also tested it a bit in sim, though it is still WIP, right?

• When using open_loop: false with and without your changes: robot is doing big snake motion around the straight path. sometimes it becomes unstable and fully veers off away from the path, potentially getting stuck circling forever. This is also what I had last year without the open loop mode or without PR 5266 (I think with the delay and slow steering, the odometry is just lagging so much behind that MPPI cannot be "dynamic" enough to follow a path)

• With open_loop: true and your fix, I am still getting the "wobble" that I had with Unclamp noise velocity. #5266 - very noisy wz command causing the optimal trajectory to jump around a lot

For comparison, without your changes:

For refernce, an extract of my config

controller_frequency: 20.0
time_steps: 100
model_dt: 0.05
batch_size: 2000

      ax_max: 1.7   # acceleration, regardless of direction
      ax_min: -1.7  # deceleration, regardless of direction
      ay_max: 0.0
      ay_min: 0.0
      az_max: 0.6061
      vx_max: 0.8
      vx_min: -0.4
      vy_max: 0.0
      wz_max: 0.853

Thanks for the feedback - what about pulling in the changes of #6066 to that branch, does that solve your "snaking" issue? I think that's probably due to a delay in the system which this could model. Using your plots, find the dt between the odom and the cmd_vel, then put that in the model delay parameter. Try again with open vs closed loop :-)

I personally hope that open loop is just totally unnecessary to get good behavior. I would leave it in place (some rare applications have no odometry at all), but my aim would be to have something that uses your real data to make sure you're physically grounded.

You're only seeing that behavior with low accelerations, correct?

@adivardi @SteveMacenski , As I remember correcly, if applying this PR, user have to make there base controller (motor driver controller) to have control cmd clamp(which sent by nav2 stack) with exact acceleration clamp in the MPPI in the driver.
For example if I set ax_max = 0.5m/s^2 for MPPI, I have to do exact clamp in the base controller. Else it could not work correctly.

Read the "Description of how this change was tested" in this PR could tell.

The version in my branch is different; the acceleration limits are applied in the refinement of the optimal trajectory 🙂

Thanks for the feedback - what about pulling in the changes of #6066 to that branch, does that solve your "snaking" issue? I think that's probably due to a delay in the system which this could model. Using your plots, find the dt between the odom and the cmd_vel, then put that in the model delay parameter. Try again with open vs closed loop :-)

I personally hope that open loop is just totally unnecessary to get good behavior. I would leave it in place (some rare applications have no odometry at all), but my aim would be to have something that uses your real data to make sure you're physically grounded.

oh nice, I wasn't aware of it. We have also been working on something similar since our robot has a really high delay.

I tried it out. It improves the oscillations but still oscillates a lot more than with the open_loop. That being said, it also improves the performance with open_loop, so a good addition regardless of the open_loop choice.

You're only seeing that behavior with low accelerations, correct?
the oscillations? yes, together with delay. When I had perfect sim, MPPI was working well. But I needed open_loop to make it work with the actual robot since it has high actuation delay + slow steering speed (=> low jerk => low accel). once I added actuation delay to the sim, I started getting the same results

I haven't tried again with your low_acceleration branch, since I see you are still working on it.

The low acceleration branch is now done and ready for testing!

So I tested again in simulation and I am still getting the very noisy/wobbly command signal.

• Adding the delay compensation PR: Always helpful, whatever the other settings are ✔️

• closed_loop: Together with the delay compensation, I have no oscillations anymore, but it gets stuck before the u-turn for some reason

• noisy command: I tried reducing wz_std. Originally it was 0.4rad/s2 (default). I reduced all the way to 0.1rad/s2, but at this value the path tracking is worse. At 0.2rad/s2, path tracking is OK. Anyway, neither change helps with the noisy signal.

Here is a comparison:
Without the low_accel branch:

With the branch:

you can see how the blue command from mppi is really noisy in the 2nd image. Also the slow down before the u-turn is much more jumpy (maybe can be improved with tuning ax a bit, but still)

And with open-loop these are removed in either/both cases? It sounds like in your application its worth sticking with that one then. I'm curious if you have any ideas why that is or something I can do to improve it? It seems to me maybe its due to noisy odometry -- else I"m not sure what else is the difference between open and closed loop. The two contributors that come to my mind are (1) delay (which we've modeled) and (2) noise (which we haven't).

And with open-loop these are removed in either/both cases? It sounds like in your application its worth sticking with that one then. I'm curious if you have any ideas why that is or something I can do to improve it? It seems to me maybe its due to noisy odometry -- else I"m not sure what else is the difference between open and closed loop. The two contributors that come to my mind are (1) delay (which we've modeled) and (2) noise (which we haven't).

No, it happens once I add the low_accel branch, whatever else I do, even in open loop. It is the same issue I had when adding this PR (5266).
It also happens when I remove the delay from sim. I also using the Gazebo ground truth for odometry.

I tried keeping all your other changes, but clamping the raw controls (state.cvx/cwz) in motion_models (so just undo your changes tho this 1 file, see diff below) - this solves the issue. So it really is caused by unclamping the raw controls, but I am not sure why.

--- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp
@@ -92,13 +92,15 @@ public:
         0).select(
         state.vx.col(i - 1) + max_delta_vx,
         state.vx.col(i - 1) - min_delta_vx);
-      state.vx.col(i) = state.cvx.col(i - 1)
-        .cwiseMax(lower_bound_vx)
-        .cwiseMin(upper_bound_vx);
+      state.cvx.col(i - 1) = state.cvx.col(i - 1)
+                                 .cwiseMax(lower_bound_vx)
+                                 .cwiseMin(upper_bound_vx);
+      state.vx.col(i) = state.cvx.col(i - 1);
 
-      state.wz.col(i) = state.cwz.col(i - 1)
-        .cwiseMax(state.wz.col(i - 1) - max_delta_wz)
-        .cwiseMin(state.wz.col(i - 1) + max_delta_wz);
+      state.cwz.col(i - 1) = state.cwz.col(i - 1)
+                                 .cwiseMax(state.wz.col(i - 1) - max_delta_wz)
+                                 .cwiseMin(state.wz.col(i - 1) + max_delta_wz);
+      state.wz.col(i) = state.cwz.col(i - 1);
 
       if (is_holo) {
         auto lower_bound_vy = (state.vy.col(i - 1) >
@@ -109,9 +111,10 @@ public:
           0).select(
           state.vy.col(i - 1) + max_delta_vy,
           state.vy.col(i - 1) - min_delta_vy);
-        state.vy.col(i) = state.cvy.col(i - 1)
-          .cwiseMax(lower_bound_vy)
-          .cwiseMin(upper_bound_vy);
+        state.cvy.col(i - 1) = state.cvy.col(i - 1)
+                                   .cwiseMax(lower_bound_vy)
+                                   .cwiseMin(upper_bound_vy);
+        state.vy.col(i) = state.cvy.col(i - 1);
       }
     }

I have no earthly idea either, I can't reproduce it. Do you you see it running in the turtlebot sim in nav2_bringup? If not, then maybe you can highlight some of the changes you have in your simulator (use the diff drive plugin, modeled delay or anything else?) which may be able to narrow down to some way I can reproduce and debug.

Push comes to shove, I can parameterize whether to clamp the controls so you can still get that behavior, but I would like to be able to reproduce and understand why (and hopefully fix) rather than introducing yet another variable and not having very clear documentation about who should use it when/why.